#ifndef __MATRIX4__
#define __MATRIX4__

#include <exception>
#include <cfloat>
#include "Point3.h"
#include "Point4.h"

template<typename T>
struct Matrix4
{
public:
	typedef Point4<T> PointT;

	float	Values[16];

	float &operator[] (const int &index)
	{
		return Values[index];
	}

	const float &operator[] (const int &index) const
	{
		return Values[index];
	}

	Matrix4()
	{
		identity();
	}

	void identity()
	{
		Values[0] = 1;
		Values[1] = 0;
		Values[2] = 0;
		Values[3] = 0;
		Values[4] = 0;
		Values[5] = 1;
		Values[6] = 0;
		Values[7] = 0;
		Values[8] = 0;
		Values[9] = 0;
		Values[10] = 1;
		Values[11] = 0;
		Values[12] = 0;
		Values[13] = 0;
		Values[14] = 0;
		Values[15] = 1;
	};

	bool invert()
	{
		T a00 = Values[0], a01 = Values[1], a02 = Values[2], a03 = Values[3],
			a10 = Values[4], a11 = Values[5], a12 = Values[6], a13 = Values[7],
			a20 = Values[8], a21 = Values[9], a22 = Values[10], a23 = Values[11],
			a30 = Values[12], a31 = Values[13], a32 = Values[14], a33 = Values[15],

			b00 = a00 * a11 - a01 * a10,
			b01 = a00 * a12 - a02 * a10,
			b02 = a00 * a13 - a03 * a10,
			b03 = a01 * a12 - a02 * a11,
			b04 = a01 * a13 - a03 * a11,
			b05 = a02 * a13 - a03 * a12,
			b06 = a20 * a31 - a21 * a30,
			b07 = a20 * a32 - a22 * a30,
			b08 = a20 * a33 - a23 * a30,
			b09 = a21 * a32 - a22 * a31,
			b10 = a21 * a33 - a23 * a31,
			b11 = a22 * a33 - a23 * a32,

			// Calculate the determinant
			det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06;

		if (!det)
			return false;

		det = 1.0 / det;

		Values[0] = (a11 * b11 - a12 * b10 + a13 * b09) * det;
		Values[1] = (a02 * b10 - a01 * b11 - a03 * b09) * det;
		Values[2] = (a31 * b05 - a32 * b04 + a33 * b03) * det;
		Values[3] = (a22 * b04 - a21 * b05 - a23 * b03) * det;
		Values[4] = (a12 * b08 - a10 * b11 - a13 * b07) * det;
		Values[5] = (a00 * b11 - a02 * b08 + a03 * b07) * det;
		Values[6] = (a32 * b02 - a30 * b05 - a33 * b01) * det;
		Values[7] = (a20 * b05 - a22 * b02 + a23 * b01) * det;
		Values[8] = (a10 * b10 - a11 * b08 + a13 * b06) * det;
		Values[9] = (a01 * b08 - a00 * b10 - a03 * b06) * det;
		Values[10] = (a30 * b04 - a31 * b02 + a33 * b00) * det;
		Values[11] = (a21 * b02 - a20 * b04 - a23 * b00) * det;
		Values[12] = (a11 * b07 - a10 * b09 - a12 * b06) * det;
		Values[13] = (a00 * b09 - a01 * b07 + a02 * b06) * det;
		Values[14] = (a31 * b01 - a30 * b03 - a32 * b00) * det;
		Values[15] = (a20 * b03 - a21 * b01 + a22 * b00) * det;

		return true;
	};

	void multiply(const Matrix4 &b)
	{
		T a00 = Values[0], a01 = Values[1], a02 = Values[2], a03 = Values[3],
			a10 = Values[4], a11 = Values[5], a12 = Values[6], a13 = Values[7],
			a20 = Values[8], a21 = Values[9], a22 = Values[10], a23 = Values[11],
			a30 = Values[12], a31 = Values[13], a32 = Values[14], a33 = Values[15];

		// Cache only the current line of the second matrix
		T b0 = b[0], b1 = b[1], b2 = b[2], b3 = b[3];
		Values[0] = b0*a00 + b1*a10 + b2*a20 + b3*a30;
		Values[1] = b0*a01 + b1*a11 + b2*a21 + b3*a31;
		Values[2] = b0*a02 + b1*a12 + b2*a22 + b3*a32;
		Values[3] = b0*a03 + b1*a13 + b2*a23 + b3*a33;

		b0 = b[4]; b1 = b[5]; b2 = b[6]; b3 = b[7];
		Values[4] = b0*a00 + b1*a10 + b2*a20 + b3*a30;
		Values[5] = b0*a01 + b1*a11 + b2*a21 + b3*a31;
		Values[6] = b0*a02 + b1*a12 + b2*a22 + b3*a32;
		Values[7] = b0*a03 + b1*a13 + b2*a23 + b3*a33;

		b0 = b[8]; b1 = b[9]; b2 = b[10]; b3 = b[11];
		Values[8] = b0*a00 + b1*a10 + b2*a20 + b3*a30;
		Values[9] = b0*a01 + b1*a11 + b2*a21 + b3*a31;
		Values[10] = b0*a02 + b1*a12 + b2*a22 + b3*a32;
		Values[11] = b0*a03 + b1*a13 + b2*a23 + b3*a33;

		b0 = b[12]; b1 = b[13]; b2 = b[14]; b3 = b[15];
		Values[12] = b0*a00 + b1*a10 + b2*a20 + b3*a30;
		Values[13] = b0*a01 + b1*a11 + b2*a21 + b3*a31;
		Values[14] = b0*a02 + b1*a12 + b2*a22 + b3*a32;
		Values[15] = b0*a03 + b1*a13 + b2*a23 + b3*a33;
	};

	void translate(const Point3<T> &v)
	{
		T x = v.X, y = v.Y, z = v.Z,
			a00, a01, a02, a03,
			a10, a11, a12, a13,
			a20, a21, a22, a23;

		Values[12] = Values[0] * x + Values[4] * y + Values[8] * z + Values[12];
		Values[13] = Values[1] * x + Values[5] * y + Values[9] * z + Values[13];
		Values[14] = Values[2] * x + Values[6] * y + Values[10] * z + Values[14];
		Values[15] = Values[3] * x + Values[7] * y + Values[11] * z + Values[15];
	};

	void scale(const Point3<T> &v)
	{
		T x = v.X, y = v.Y, z = v.Z;

		Values[0] = Values[0] * x;
		Values[1] = Values[1] * x;
		Values[2] = Values[2] * x;
		Values[3] = Values[3] * x;
		Values[4] = Values[4] * y;
		Values[5] = Values[5] * y;
		Values[6] = Values[6] * y;
		Values[7] = Values[7] * y;
		Values[8] = Values[8] * z;
		Values[9] = Values[9] * z;
		Values[10] = Values[10] * z;
		Values[11] = Values[11] * z;
		Values[12] = Values[12];
		Values[13] = Values[13];
		Values[14] = Values[14];
		Values[15] = Values[15];
	};

	/**
	* Rotates a mat4 by the given angle
	*
	* @param {mat4} out the receiving matrix
	* @param {mat4} a the matrix to rotate
	* @param {Number} rad the angle to rotate the matrix by
	* @param {vec3} axis the axis to rotate around
	* @returns {mat4} out
	*/
	bool rotate(const T &rad, const Point3i &axis)
	{
		T x = axis.X, y = axis.Y, z = axis.Z,
			len = sqrtf(x * x + y * y + z * z),
			s, c, t,
			a00, a01, a02, a03,
			a10, a11, a12, a13,
			a20, a21, a22, a23,
			b00, b01, b02,
			b10, b11, b12,
			b20, b21, b22;

		if (fabsf(len) < FLT_EPSILON) { return false; }

		len = 1 / len;
		x *= len;
		y *= len;
		z *= len;

		s = sinf(rad);
		c = cosf(rad);
		t = 1 - c;

		a00 = Values[0]; a01 = Values[1]; a02 = Values[2]; a03 = Values[3];
		a10 = Values[4]; a11 = Values[5]; a12 = Values[6]; a13 = Values[7];
		a20 = Values[8]; a21 = Values[9]; a22 = Values[10]; a23 = Values[11];

		// Construct the elements of the rotation matrix
		b00 = x * x * t + c; b01 = y * x * t + z * s; b02 = z * x * t - y * s;
		b10 = x * y * t - z * s; b11 = y * y * t + c; b12 = z * y * t + x * s;
		b20 = x * z * t + y * s; b21 = y * z * t - x * s; b22 = z * z * t + c;

		// Perform rotation-specific matrix multiplication
		Values[0] = a00 * b00 + a10 * b01 + a20 * b02;
		Values[1] = a01 * b00 + a11 * b01 + a21 * b02;
		Values[2] = a02 * b00 + a12 * b01 + a22 * b02;
		Values[3] = a03 * b00 + a13 * b01 + a23 * b02;
		Values[4] = a00 * b10 + a10 * b11 + a20 * b12;
		Values[5] = a01 * b10 + a11 * b11 + a21 * b12;
		Values[6] = a02 * b10 + a12 * b11 + a22 * b12;
		Values[7] = a03 * b10 + a13 * b11 + a23 * b12;
		Values[8] = a00 * b20 + a10 * b21 + a20 * b22;
		Values[9] = a01 * b20 + a11 * b21 + a21 * b22;
		Values[10] = a02 * b20 + a12 * b21 + a22 * b22;
		Values[11] = a03 * b20 + a13 * b21 + a23 * b22;

		return true;
	};

	void perspective(const T &fovy, const T &aspect, const T &near, const T &far)
	{
		T f = 1.0 / tanf(fovy / 2),
			nf = 1.0 / (near - far);
		Values[0] = f / aspect;
		Values[1] = 0;
		Values[2] = 0;
		Values[3] = 0;
		Values[4] = 0;
		Values[5] = f;
		Values[6] = 0;
		Values[7] = 0;
		Values[8] = 0;
		Values[9] = 0;
		Values[10] = (far + near) * nf;
		Values[11] = -1;
		Values[12] = 0;
		Values[13] = 0;
		Values[14] = (2 * far * near) * nf;
		Values[15] = 0;
	};

	void ortho(const int &left, const int &right, const int &bottom, const int &top, const T &near, const T &far)
	{
		T lr = 1.0f / (left - right),
			bt = 1.0f / (bottom - top),
			nf = 1.0f / (near - far);
		Values[0] = -2 * lr;
		Values[1] = 0;
		Values[2] = 0;
		Values[3] = 0;
		Values[4] = 0;
		Values[5] = -2 * bt;
		Values[6] = 0;
		Values[7] = 0;
		Values[8] = 0;
		Values[9] = 0;
		Values[10] = 2 * nf;
		Values[11] = 0;
		Values[12] = (left + right) * lr;
		Values[13] = (top + bottom) * bt;
		Values[14] = (far + near) * nf;
		Values[15] = 1;
	};

	Point3<T> transformVec3(const Point3<T> &a) const
	{
		T x = a.X, y = a.Y, z = a.Z;

		return Point3<T>(
			Values[0] * x + Values[4] * y + Values[8] * z + Values[12],
			Values[1] * x + Values[5] * y + Values[9] * z + Values[13],
			Values[2] * x + Values[6] * y + Values[10] * z + Values[14]
		);
	};

	Point4<T> transformVec4(const Point4<T> &a) const
	{
		T x = a.X, y = a.Y, z = a.Z, w = a.W;

		return Point4<T>(
			Values[0] * x + Values[4] * y + Values[8] * z + Values[12] * w,
			Values[1] * x + Values[5] * y + Values[9] * z + Values[13] * w,
			Values[2] * x + Values[6] * y + Values[10] * z + Values[14] * w,
			Values[3] * x + Values[7] * y + Values[11] * z + Values[15] * w
			);
	};
};

typedef Matrix4<float> Matrix4f;
#endif